#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <std_msgs/msg/bool.hpp>
#include <cmath>
#include <fstream>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>


#include <nlohmann/json.hpp>
using json = nlohmann::json;
#define M_PI 3.14159265358979323846

// 从 JSON 文件中读取普通params
double read_normal_params_from_json(const std::string& filename,std::string key_name="kp") {
    double params;

    // 打开 JSON 文件
    std::ifstream file(filename);
    if (!file.is_open()) {
        throw std::runtime_error("Failed to open JSON file: " + filename);
    }

    // 解析 JSON
    json j;
    file >> j;

    params = j[key_name];

    return params;
}

class PIDController : public rclcpp::Node {
public:
    PIDController()
        : Node("airsim_control_pid"), target_received_(false), goal_reached_(false), goal_reached_received_(false) {
        // 创建订阅器
        target_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            "/airsim/target_point", 10,
            std::bind(&PIDController::on_target_received, this, std::placeholders::_1));

        pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            "/airsim/pose", 10,
            std::bind(&PIDController::on_pose_received, this, std::placeholders::_1));

        goal_reached_sub_ = this->create_subscription<std_msgs::msg::Bool>(
            "/airsim/goal_reached_received", 10,
            std::bind(&PIDController::on_goal_reached_received, this, std::placeholders::_1));

        // 创建发布器
        cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/airsim/cmd_vel", 10);
        goal_reached_pub_ = this->create_publisher<std_msgs::msg::Bool>("/airsim/goal_reached", 10);

        // 定时器，用于控制主循环
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(50),
            std::bind(&PIDController::control_loop, this));
        std::string package_share_directory = SRC_PATH;
        std::cout << "package_share_directory: " << package_share_directory << std::endl;
        std::string param_path_ = package_share_directory + "/config/params.json";
        // 初始化 PID 参数
        kp_ = read_normal_params_from_json(param_path_, "kp");
        ki_ = read_normal_params_from_json(param_path_, "ki");
        kd_ = read_normal_params_from_json(param_path_, "kd");
        error_sum_x_ = error_sum_y_ = error_sum_z_ = 0.0;
        last_error_x_ = last_error_y_ = last_error_z_ = 0.0;
    }

private:
    // 接收到目标点时的回调
    void on_target_received(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
        target_ = *msg;
        target_received_ = true;
        goal_reached_ = false;
        goal_reached_received_ = false;
        // RCLCPP_INFO(this->get_logger(), "Received target point: (%.2f, %.2f, %.2f)",
        //             target_.pose.position.x, target_.pose.position.y, target_.pose.position.z);
    }

    // 接收到无人机位姿时的回调
    void on_pose_received(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
        current_pose_ = *msg;
    }

    // 接收到 "目标到达已收到" 消息时的回调
    void on_goal_reached_received(const std_msgs::msg::Bool::SharedPtr msg) {
        if (msg->data) {
            goal_reached_received_ = true;
            RCLCPP_INFO(this->get_logger(), "Goal reached confirmation received. Stopping goal message publication.");
        }
    }

    // 控制主循环
    void control_loop() {
        // 如果未收到目标点或已确认到达，停止控制
        // if (!target_received_ || (goal_reached_ && goal_reached_received_)) {
        //     return;
        // }

        // 如果已到达目标点但未收到确认，则继续发布 "目标到达" 消息
        if (goal_reached_ && !goal_reached_received_) {
            auto goal_msg = std_msgs::msg::Bool();
            goal_msg.data = true;
            goal_reached_pub_->publish(goal_msg);
            RCLCPP_INFO(this->get_logger(), "Re-published goal reached message.");
            return;
        }

        // 计算位置误差
        double error_x = target_.pose.position.x - current_pose_.pose.position.x;
        double error_y = target_.pose.position.y - current_pose_.pose.position.y;
        double error_z = -target_.pose.position.z + current_pose_.pose.position.z;

        double distance = std::sqrt(error_x * error_x + error_y * error_y + error_z * error_z);

        // 判断是否到达目标点
        if (distance < 0.5) {  // 到达阈值
            RCLCPP_INFO(this->get_logger(), "Goal reached. Publishing goal reached message.");
            auto goal_msg = std_msgs::msg::Bool();
            goal_msg.data = true;
            goal_reached_pub_->publish(goal_msg);
            goal_reached_ = true;
            return;
        }

        // PID 控制计算
        // x方向保持固定速度
        // double vel_x = std::min(2.0, pid_compute(error_x, error_sum_x_, last_error_x_));  // 固定速度
        double vel_x = 2.0;  // 固定速度
        // double vel_y = pid_compute(error_y, error_sum_y_, last_error_y_);  // 控制y方向
        double vel_y = 0.0;  // 控制y方向
        double vel_z = pid_compute(error_z, error_sum_z_, last_error_z_);  // 控制z方向

        // 计算当前角度 (Yaw)
        tf2::Quaternion q(
            current_pose_.pose.orientation.x,
            current_pose_.pose.orientation.y,
            current_pose_.pose.orientation.z,
            current_pose_.pose.orientation.w
        );
        tf2::Matrix3x3 m(q);

        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);
        double current_angle = yaw;//获取当前yaw角度
        // 如果需要角度控制，可以通过目标点与当前位置的偏差计算
        // double target_angle = std::atan2(target_.pose.position.y - current_pose_.pose.position.y,
        //                                 target_.pose.position.x - current_pose_.pose.position.x);
        double target_angle = std::atan2(target_.pose.position.y - current_pose_.pose.position.y,
                                 target_.pose.position.x - current_pose_.pose.position.x);
        double target_angle_degrees = target_angle;

        // double current_angle = tf2::getYaw(current_pose_.pose.orientation);
        double error_angle = target_angle - current_angle;

        double vel_angle = target_angle * 180.0 / M_PI;  // 角度控制
        // double vel_angle = pid_compute(error_angle, error_sum_angle_, last_error_angle_)*5.0;  // 控制角速度

        // 将四元数转换为旋转矩阵
        tf2::Matrix3x3 rotation_matrix(q);

        // 机体坐标系下的速度向量
        tf2::Vector3 velocity_body(vel_x, vel_y, vel_z);

        // 将速度向量从机体坐标系转换到世界坐标系
        tf2::Vector3 velocity_world = rotation_matrix * velocity_body;

        // 发布速度命令
        auto cmd_msg = geometry_msgs::msg::Twist();
        cmd_msg.linear.x = velocity_world.x(); 
        cmd_msg.linear.y = velocity_world.y();  
        cmd_msg.linear.z = velocity_world.z();  
        cmd_msg.angular.z = vel_angle;  // 控制角度
        cmd_vel_pub_->publish(cmd_msg);

        RCLCPP_INFO(this->get_logger(), "Published velocity: (x: %.2f, y: %.2f, z: %.2f, angular.z: %.2f)", 
                    vel_x, vel_y, vel_z, vel_angle);
    }

    // PID 控制器计算
    double pid_compute(double error, double &error_sum, double &last_error) {
        error_sum += error;  // 累积误差
        double derivative = error - last_error;
        last_error = error;
        return kp_ * error + ki_ * error_sum + kd_ * derivative;
    }

    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr target_sub_;
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr goal_reached_sub_;

    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr goal_reached_pub_;

    rclcpp::TimerBase::SharedPtr timer_;

    geometry_msgs::msg::PoseStamped target_;
    geometry_msgs::msg::PoseStamped current_pose_;

    bool target_received_;
    bool goal_reached_;
    bool goal_reached_received_;

    // PID 参数
    double kp_, ki_, kd_;
    double error_sum_x_, error_sum_y_, error_sum_z_;
    double last_error_x_, last_error_y_, last_error_z_;
    double error_sum_angle_, last_error_angle_;
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<PIDController>());
    rclcpp::shutdown();
    return 0;
}
